/*
 * mpcc.c
 *
 *  Created on: Apr 1, 2024
 *      Author: Yiwen Zhu
 */
#include "mpdtc.h"
#include <stdio.h>
#include "math.h"
#include "string.h"
#include "Motor_Control_Fcns.h"
#include "Motor_Control_Init.h"


#define HRTIM_ZERO_CMP	(0)
#define HRTIM_FULL_CMP	(DEF_SVM_TO_CMP)

// Sort for non-weight factor
static void bubble_sort(float arr[], int index[], int n) {
    int i, j;
    float temp;
    for (i = 0; i < n-1; i++) {
        for (j = 0; j < n-i-1; j++) {
            if (arr[j] > arr[j+1]) {
                // Swap values
                temp = arr[j];
                arr[j] = arr[j+1];
                arr[j+1] = temp;
                // Swap indices
                int temp_index = index[j];
                index[j] = index[j+1];
                index[j+1] = temp_index;
            }
        }
    }
}

static const char State[7][3] = {
		{0, 0, 0},	// State V0/V7
		{1, 0, 0},
		{1, 1, 0},
		{0, 1, 0},
		{0, 1, 1},
		{0, 0, 1},
		{1, 0, 1},
};

void mpdtc_param_update_online(void){
	if(DEF_CTL_STATE==RUN){
	 }
}

void mpdtc_init(void){
	mpdtc1.Udc = AD_BUF[2]*DEF_AD_TO_REAL_VDC;

	mpdtc1.Val[0] = 0.0f;
	mpdtc1.Val[1] = mpdtc1.Udc*0.5443f;
	mpdtc1.Val[2] = mpdtc1.Udc*0.2722f;
	mpdtc1.Val[3] = mpdtc1.Udc*-0.2722f;
	mpdtc1.Val[4] = mpdtc1.Udc*-0.5443f;
	mpdtc1.Val[5] = mpdtc1.Udc*-0.2722f;
	mpdtc1.Val[6] = mpdtc1.Udc*0.2722f;

	mpdtc1.Vbe[0] = 0.0f;
	mpdtc1.Vbe[1] = 0.0f;
	mpdtc1.Vbe[2] = mpdtc1.Udc*0.4714f;
	mpdtc1.Vbe[3] = mpdtc1.Udc*0.4714f;
	mpdtc1.Vbe[4] = 0.0f;
	mpdtc1.Vbe[5] = mpdtc1.Udc*-0.4714f;
	mpdtc1.Vbe[6] = mpdtc1.Udc*-0.4714f;


	mpdtc1.K1 = mpdtc1.Ts /mpdtc1.Ld;
	mpdtc1.K2 = mpdtc1.Rs /mpdtc1.Ld *mpdtc1.Ts;
	mpdtc1.K3 = mpdtc1.Lq /mpdtc1.Ld *mpdtc1.Ts;

	mpdtc1.K4 = mpdtc1.Ts /mpdtc1.Lq;
	mpdtc1.K5 = mpdtc1.Rs /mpdtc1.Lq *mpdtc1.Ts;
	mpdtc1.K6 = mpdtc1.Ld /mpdtc1.Lq *mpdtc1.Ts;
	mpdtc1.K7 = mpdtc1.Psif /mpdtc1.Lq *mpdtc1.Ts;
}


void mpdtc_task(void){
	static int Ind_Last = 0;
//	static int Loop_Index = 0;

	float id_kp1,iq_kp1,id_kp2,iq_kp2,psi_d_kp2,psi_q_kp2;
	float temp1,temp2;
	float Vd,Vq;
	int Ind=0;
	float g;
	float gmin = 1000.0f; // Must large enough!

	// One-step delay compensation
	id_kp1 = mpdtc1.Fdb_Id - mpdtc1.K2*mpdtc1.Fdb_Id  + mpdtc1.K1*mpdtc1.Vd_Last + mpdtc1.K3*mpdtc1.Fdb_Iq*mpdtc1.We; //
	iq_kp1 = mpdtc1.Fdb_Iq - mpdtc1.K5*mpdtc1.Fdb_Iq  + mpdtc1.K4*mpdtc1.Vq_Last + mpdtc1.K6*mpdtc1.Fdb_Id*mpdtc1.We -mpdtc1.K7*mpdtc1.We ; //

	// Pre-calculate the common part
	temp1 = id_kp1 - mpdtc1.K2*id_kp1 + mpdtc1.K3*iq_kp1*mpdtc1.We;
	temp2 = iq_kp1 - mpdtc1.K5*iq_kp1 -mpdtc1.K7*mpdtc1.We +mpdtc1.K6*id_kp1*mpdtc1.We;

	// Search the "optimal" voltage vector
	for(int i=0;i<7;i++){
		Vd = mpdtc1.cos*mpdtc1.Val[i] +mpdtc1.sin*mpdtc1.Vbe[i];
		Vq = -mpdtc1.sin*mpdtc1.Val[i] +mpdtc1.cos*mpdtc1.Vbe[i];
		id_kp2 = temp1 + mpdtc1.K1*Vd ;	 //
		iq_kp2 = temp2 + mpdtc1.K4*Vq ; //

		psi_d_kp2 = mpdtc1.Ld*id_kp2 + mpdtc1.Psif;
		psi_q_kp2 =	mpdtc1.Lq*iq_kp2;
		mpdtc1.Fdb_Flux = sqrtf(psi_d_kp2*psi_d_kp2 +psi_q_kp2*psi_q_kp2);
		// Const-Power transform  1*Trque, Const-Amplitude transform 1.5*Trque
		mpdtc1.Fdb_Trq = mpdtc1.Np*( psi_d_kp2*iq_kp2 -psi_q_kp2*id_kp2);

		g = mpdtc1.Weight_Flux*fabsf(mpdtc1.Ref_Trq -mpdtc1.Fdb_Trq)+ fabsf(mpdtc1.Ref_Flux -mpdtc1.Fdb_Flux) ;

		if(g<=gmin){
			gmin = g;
			Ind = i;
			mpdtc1.Vd_Last = Vd;
			mpdtc1.Vq_Last = Vq;
		}
	}

//	if(Loop_Index++>=2)	Loop_Index = 0;
//	Ind = Loop_Index;

	// If the switching state is 1，set a higher compare value
	if(State[Ind][0]==1)	mpdtc1.Ta = HRTIM_FULL_CMP;
	else					mpdtc1.Ta = HRTIM_ZERO_CMP;

	if(State[Ind][1]==1)	mpdtc1.Tb = HRTIM_FULL_CMP;
	else					mpdtc1.Tb = HRTIM_ZERO_CMP;

	if(State[Ind][2]==1)	mpdtc1.Tc = HRTIM_FULL_CMP;
	else					mpdtc1.Tc = HRTIM_ZERO_CMP;

	// record the last period value
	mpdtc1.Index_Last2 = mpdtc1.Index_Last;
	mpdtc1.Index_Last = Ind_Last;
	Ind_Last = Ind;
	mpdtc1.g_opt = gmin;
}

void mpdtc_evenhand_task(void){
	static int Ind_Last = 0;
//	static int Loop_Index = 0;

	float id_kp1,iq_kp1,id_kp2,iq_kp2,psi_d_kp2,psi_q_kp2;
	float temp1,temp2;
	float Vd,Vq;
	int Ind=99;
	float gmin = 1000.0f; // Must large enough!
	float vd[7]={0},vq[7]={0};
	float g_trq[7]={0} ,g_flux[7]={0};
	int ind_trq[7] = {0};
	int ind_flux[7] = {0};

	// One-step delay compensation
	id_kp1 = mpdtc1.Fdb_Id - mpdtc1.K2*mpdtc1.Fdb_Id  + mpdtc1.K1*mpdtc1.Vd_Last + mpdtc1.K3*mpdtc1.Fdb_Iq*mpdtc1.We; //
	iq_kp1 = mpdtc1.Fdb_Iq - mpdtc1.K5*mpdtc1.Fdb_Iq  + mpdtc1.K4*mpdtc1.Vq_Last + mpdtc1.K6*mpdtc1.Fdb_Id*mpdtc1.We -mpdtc1.K7*mpdtc1.We ; //

	// Pre-calculate the common part
	temp1 = id_kp1 - mpdtc1.K2*id_kp1 + mpdtc1.K3*iq_kp1*mpdtc1.We;
	temp2 = iq_kp1 - mpdtc1.K5*iq_kp1 -mpdtc1.K7*mpdtc1.We +mpdtc1.K6*id_kp1*mpdtc1.We;

	// #######################  Start of the for loop ###########################
	// Search the "optimal" voltage vector
	for(int i=0;i<7;i++){
		Vd = mpdtc1.cos*mpdtc1.Val[i] +mpdtc1.sin*mpdtc1.Vbe[i];
		Vq = -mpdtc1.sin*mpdtc1.Val[i] +mpdtc1.cos*mpdtc1.Vbe[i];
		vd[i] = Vd;
		vq[i] = Vq;
		id_kp2 = temp1 + mpdtc1.K1*Vd ;	 //
		iq_kp2 = temp2 + mpdtc1.K4*Vq ; //

		psi_d_kp2 = mpdtc1.Ld*id_kp2 + mpdtc1.Psif;
		psi_q_kp2 =	mpdtc1.Lq*iq_kp2;
		mpdtc1.Fdb_Flux = (psi_d_kp2*psi_d_kp2 +psi_q_kp2*psi_q_kp2);
		// Const-Power transform  1*Trque, Const-Amplitude transform 1.5*Trque
		mpdtc1.Fdb_Trq = mpdtc1.Np*( psi_d_kp2*iq_kp2 -psi_q_kp2*id_kp2);

		g_trq[i]  = fabsf(mpdtc1.Ref_Trq -mpdtc1.Fdb_Trq);
		g_flux[i] = fabsf(mpdtc1.Ref_Flux*mpdtc1.Ref_Flux -mpdtc1.Fdb_Flux) ;
		ind_trq[i] = i;
		ind_flux[i] = i;
	}
	// #######################  end of the for loop ###########################

	// Sort the two error and index
	bubble_sort(g_trq,ind_trq,7);
	bubble_sort(g_flux,ind_flux,7);
	int cross_ind[4]={0};	// [ind]
	float cross_rank_sum[4]={0};//[rank_sum]

	// Select the optimum vector
	for(int i=0;i<4;i++){
		for(int j=0;j<7;j++){
			if( ind_flux[i] == ind_trq[j] ){
				cross_ind[i] = ind_flux[i];
				cross_rank_sum[i] = i+j;
				break;
			}
		}
	}
	bubble_sort(cross_rank_sum,cross_ind,4);
	Ind = cross_ind[0];


	// If the switching state is 1，set a higher compare value
	if(State[Ind][0]==1)	mpdtc1.Ta = HRTIM_FULL_CMP;
	else					mpdtc1.Ta = HRTIM_ZERO_CMP;

	if(State[Ind][1]==1)	mpdtc1.Tb = HRTIM_FULL_CMP;
	else					mpdtc1.Tb = HRTIM_ZERO_CMP;

	if(State[Ind][2]==1)	mpdtc1.Tc = HRTIM_FULL_CMP;
	else					mpdtc1.Tc = HRTIM_ZERO_CMP;

	// record the last period value
	mpdtc1.Index_Last2 = mpdtc1.Index_Last;
	mpdtc1.Index_Last = Ind_Last;
	Ind_Last = Ind;
	mpdtc1.g_opt = gmin;
	mpdtc1.Vd_Last = vd[Ind];
	mpdtc1.Vq_Last = vq[Ind];
}


